import cv2
import serial
import time
import numpy as np

# 尝试初始化串口
ser = None
try:
    ser = serial.Serial(
    port='/dev/ttyAMA0',
    baudrate=115200,
    timeout=1
    )
    if ser:
        print("串口已连接")
    else:
        print("警告：串口连接失败")
except:
    print("警告：无法连接串口，将继续运行但不发送数据")

# 加载多角度人脸检测分类器
face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')
profile_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_profileface.xml')

# 初始化摄像头
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

while True:
    # 读取摄像头帧
    ret, frame = cap.read()
    if not ret:
        break

    # 转换为灰度图像
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 检测正脸和侧脸
    faces = face_cascade.detectMultiScale(gray, 1.1, 4, minSize=(30, 30))
    profiles = profile_cascade.detectMultiScale(gray, 1.1, 4, minSize=(30, 30))
    
    # 合并检测结果并去重
    def non_max_suppression(boxes, overlapThresh=0.3):
        if len(boxes) == 0:
            return []
        
        boxes = np.array(boxes)
        pick = []
        
        x1 = boxes[:,0]
        y1 = boxes[:,1]
        x2 = x1 + boxes[:,2]
        y2 = y1 + boxes[:,3]
        
        area = boxes[:,2] * boxes[:,3]
        idxs = np.argsort(area)
        
        while len(idxs) > 0:
            last = len(idxs) - 1
            i = idxs[last]
            pick.append(i)
            
            xx1 = np.maximum(x1[i], x1[idxs[:last]])
            yy1 = np.maximum(y1[i], y1[idxs[:last]])
            xx2 = np.minimum(x2[i], x2[idxs[:last]])
            yy2 = np.minimum(y2[i], y2[idxs[:last]])
            
            w = np.maximum(0, xx2 - xx1 + 1)
            h = np.maximum(0, yy2 - yy1 + 1)
            
            overlap = (w * h) / area[idxs[:last]]
            
            idxs = np.delete(idxs, np.concatenate(([last],
                np.where(overlap > overlapThresh)[0])))

        return boxes[pick].astype("int")

    all_faces = []
    all_faces.extend(faces)
    all_faces.extend(profiles)
    all_faces = non_max_suppression(all_faces)

    # 遍历去重后的人脸
    for (x, y, w, h) in all_faces:
        # 绘制矩形框
        cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 0, 0), 2)
        
        # 计算中心坐标
        center_x = x + w//2
        center_y = y + h//2

        print(f"检测到人脸，中心坐标：({center_x}, {center_y})")
        
        # 绘制中心点
        cv2.circle(frame, (center_x, center_y), 5, (0, 255, 0), -1)
        
        # 通过串口发送坐标数据（如果串口已连接）
        if ser: #如果不是None,说明已经连接了串口
            ser.write(f"{center_x},{center_y}\n".encode())
    
    # 显示结果
    cv2.imshow('Face Detection', frame)

    # 等待10毫秒
    time.sleep(0.01)
    
    # 按'q'键退出
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break


# 释放资源
cap.release()
cv2.destroyAllWindows()
if ser:
    ser.close()
